
#ifndef VECTOR3_H
#define VECTOR3_H

#include <cmath>

template <typename Real>
class Vector3 {
	enum Axis { X, Y, Z, COMPONENT_COUNT };
	
public:
	Vector3(Real x = static_cast<Real>(0), Real y = static_cast<Real>(0), Real z = static_cast<Real>(0));
	virtual ~Vector3();
	
	Vector3<Real> cross(const Vector3<Real>& v) const;
	void normalize();
	Vector3<Real> normalized() const;

	Real angle(const Vector3<Real>& v) const;
	Real dot(const Vector3<Real>& v) const;
	Real length() const;
	Real lengthSquared() const;
	Real norm() const;
	Real normSquared() const;
	
	const Real& x() const;
	const Real& y() const;
	const Real& z() const;
	
	Real& x();
	Real& y();
	Real& z();
	
	operator const Real* const () const { return this->data; }
	
	Vector3<Real> operator - (const Vector3<Real>& v) const;
	Vector3<Real> operator + (const Vector3<Real>& v) const;
	Vector3<Real> operator * (const Real& scalar) const;
	Vector3<Real> operator * (const Vector3<Real>& v) const;

	bool operator == (const Vector3<Real>& v) const;
	bool operator != (const Vector3<Real>& v) const;
	bool operator < (const Vector3<Real>& v) const;
	
	static Real Length(const Vector3<Real>& v);
	static Real LengthSquared(const Vector3<Real>& v);
	static Real Norm(const Vector3<Real>& v);
	static Real NormSquared(const Vector3<Real>& v);
	static Vector3<Real> Normalize(const Vector3<Real>& v);
	static Vector3<Real> Dot(const Vector3<Real>& a, const Vector3<Real>& b);
	static Vector3<Real> Cross(const Vector3<Real>& a, const Vector3<Real>& b);

	static Vector3<Real> Zero();
	static Vector3<Real> UnitX();
	static Vector3<Real> UnitY();
	static Vector3<Real> UnitZ();
	
protected:
	Real data[COMPONENT_COUNT];
};

template <typename Real>
Vector3<Real>::Vector3(Real x, Real y, Real z) {
	this->data[X] = x;
	this->data[Y] = y;
	this->data[Z] = z;
}

template <typename Real>
Vector3<Real>::~Vector3() {}
	
template <typename Real>
Vector3<Real> Vector3<Real>::cross(const Vector3<Real>& v) const {
	return Vector3<Real>::Cross(*this, v);
}

template <typename Real>
void Vector3<Real>::normalize() {
	Real invLen = static_cast<Real>(1) / this->length();
	this->data[X] *= invLen;
	this->data[Y] *= invLen;
	this->data[Z] *= invLen;
}

template <typename Real>
Vector3<Real> Vector3<Real>::normalized() const {
	Real invLen = static_cast<Real>(1) / this->length();
	return Vector3<Real>(this->data[X] * invLen, this->data[Y] * invLen, this->data[Z] * invLen);
}

/* Dot product angle between vectors: theta = cos-1(A.B / |A||B|) */
template<typename Real>
inline Real Vector3<Real>::angle(const Vector3<Real>& v) const {
	return std::acos((this->data[X] * vector.data[X] + this->data[Y] * vector.data[Y] + this->data[Z] * vector.data[Z]) / (this->norm() * vector.norm()));
}

template <typename Real>
Real Vector3<Real>::dot(const Vector3<Real>& v) const {
	return this->data[X] * v.data[X] + this->data[Y] * v.data[Y] + this->data[Z] * v.data[Z];
}

template <typename Real>
Real Vector3<Real>::length() const {
	return std::sqrt(this->lengthSquared());
}

template <typename Real>
Real Vector3<Real>::lengthSquared() const {
	return std::pow(this->data[X], static_cast<Real>(2)) + std::pow(this->data[Y], static_cast<Real>(2)) + std::pow(this->data[Z], static_cast<Real>(2));
}

template <typename Real>
Real Vector3<Real>::norm() const {
	return this->length();
}

template <typename Real>
Real Vector3<Real>::normSquared() const {
	return this->lengthSquared();
}

template <typename Real>
const Real& Vector3<Real>::x() const {
	return this->data[X];
}

template <typename Real>
const Real& Vector3<Real>::y() const {
	return this->data[Y];
}

template <typename Real>
const Real& Vector3<Real>::z() const {
	return this->data[Z];
}

template <typename Real>
Real& Vector3<Real>::x() {
	return this->data[X];
}

template <typename Real>
Real& Vector3<Real>::y() {
	return this->data[Y];
}

template <typename Real>
Real& Vector3<Real>::z() {
	return this->data[Z];
}

template <typename Real>
Vector3<Real> Vector3<Real>::operator - (const Vector3<Real>& v) const {
	return Vector3<Real>(this->data[X] - v.data[X], this->data[Y] - v.data[Y], this->data[Z] - v.data[Z]);
}

template <typename Real>
Vector3<Real> Vector3<Real>::operator + (const Vector3<Real>& v) const {
	return Vector3<Real>(this->data[X] + v.data[X], this->data[Y] + v.data[Y], this->data[Z] + v.data[Z]);
}

template <typename Real>
Vector3<Real> Vector3<Real>::operator * (const Real& scalar) const {
	return Vector3<Real>(this->data[X] * scalar, this->data[Y] * scalar, this->data[Z] * scalar);
}

template<typename Real>
inline Vector3<Real> Vector3<Real>::operator * (const Vector3<Real>& v) const {
	return Vector3<Real>::Cross(*this, v);
}

template <typename Real>
bool Vector3<Real>::operator == (const Vector3<Real>& v) const {
	if ( this->data[X] == v.data[X] && this->data[Y] == v.data[Y] && this->data[Z] == v.data[Z] ) return true;
	return false;
}

template <typename Real>
bool Vector3<Real>::operator != (const Vector3<Real>& v) const {
	if ( this->data[X] != v.data[X] ) return true;
	if ( this->data[Y] != v.data[Y] ) return true;
	if ( this->data[Z] != v.data[Z] ) return true;
	return false;
}

template<typename Real>
inline bool Vector3<Real>::operator < (const Vector3<Real>& v) const {
	if ( this->norm() < v.norm() ) return true;
	return false;
}

template <typename Real>
Real Vector3<Real>::Length(const Vector3<Real>& v) {
	return std::sqrt(Vector3<Real>::LengthSquared(v));
}

template <typename Real>
Real Vector3<Real>::LengthSquared(const Vector3<Real>& v) {
	return std::pow(v.data[X], static_cast<Real>(2)) + std::pow(v.data[Y], static_cast<Real>(2)) + std::pow(v.data[Z], static_cast<Real>(2));
}

template <typename Real>
Real Vector3<Real>::Norm(const Vector3<Real>& v) {
	return Vector3<Real>::Length(v);
}

template <typename Real>
Real Vector3<Real>::NormSquared(const Vector3<Real>& v) {
	return Vector3<Real>::LengthSquared(v);
}
	
template <typename Real>
Vector3<Real> Vector3<Real>::Normalize(const Vector3<Real>& v) {
	Real invLen = static_cast<Real>(1) / Vector3<Real>::Length(v);
	return Vector3<Real>(v.data[X] * invLen, v.data[Y] * invLen, v.data[Z] * invLen);
}

/* InnerProduct = (ax * bx) + (ay * by) + (az * bz) */
template <typename Real>
Vector3<Real> Vector3<Real>::Dot(const Vector3<Real>& a, const Vector3<Real>& b) {
	return a.data[X] * b.data[X] + a.data[Y] * b.data[Y] + a.data[Z] * b.data[Z];
}

/*
 * Formal Determinant Formulation (i, j, k represent ortho axes):
 * 
 *         |  i  j  k |
 * a x b = | ax ay az |
 *         | bx by bz |
 *
 * Cofactors: 
 * i | ay az |
 *   | by bz |
 *
 * j | ax az |
 *   | bx bz |
 *
 * k | ax ay |
 *   | bx by |
 * 
 * Determinants:
 * x =  (ay * bz - az * by)i
 * y = -(ax * bz - az * bx)j
 * z =  (ax * by - ay * bx)k
 */
template <typename Real>
Vector3<Real> Vector3<Real>::Cross(const Vector3<Real>& a, const Vector3<Real>& b) {
	Vector3<Real> result;
    result.data[X] = ((a.data[Y] * b.data[Z]) - (a.data[Z] * b.data[Y]));
    result.data[Y] = -((a.data[X] * b.data[Z]) - (a.data[Z] * b.data[X]));
    result.data[Z] = ((a.data[X] * b.data[Y]) - (a.data[Y] * b.data[X]));
    return result;
}
	
template <typename Real>
Vector3<Real> Vector3<Real>::Zero() {
	return Vector3<Real>();
}

template <typename Real>
Vector3<Real> Vector3<Real>::UnitX() {
	return Vector3<Real>(static_cast<Real>(1));
}

template <typename Real>
Vector3<Real> Vector3<Real>::UnitY() {
	return Vector3<Real>(0, static_cast<Real>(1));
}

template <typename Real>
Vector3<Real> Vector3<Real>::UnitZ() {
	return Vector3<Real>(0, 0, static_cast<Real>(1));
}
	
typedef Vector3<double> Vector3d;
typedef Vector3<float> Vector3f;
typedef Vector3<int> Vector3i;
	
#endif
